The same formula is used in diff_drive_controller.

I fixed it too thanks.

Is there a way in this repo to factorize files between packages or not?

Is there a way in this repo to factorize files between packages or not?

Creating a base package... (#377)

...and now the limits test is failing.
@artivis @efernandez ?

I have fixed the tests doubling the jerk limits. If you prefer to modify end values (after the simulation), tell me.

I have the feeling that there is a notation issue, dts are not actually the same in the rightmost development of the second line.
The way I understand it, the calculation of jerk actually 'spans' two iterations of the control loop (e.g. first iteration from t-2->t-1, second from t-1->t) thus dt * 2 iterations.
I'll have a deeper look asap.

Yes I thought that the mistake came from the two iterations process (there is 2 times dt between t-2 and t).

The way I understand it, the calculation of jerk actually 'spans' two iterations of the control loop

You can see it a different way if you define the jerk as the difference between current and last acceleration. In this process it 'spans' only one dt. And the computation of each acceleration is independant (each one use one dt). That was I try to explain with the maths above.

Notice that I did not verify the formulas to find the mistake, I was plotting the jerk and saw it was doubled. Then I got back to the code and the maths.

I ran 3 times the tests and it fails due to instability of joint_trajectory_controller :

Summary: 201 tests, 0 errors, 0 failures, 0 skipped

joint_trajectory_controller/rosunit-joint_trajectory_controller_vel_test.xml: 26 tests, 0 errors, 1 failures, 0 skipped
Summary: 201 tests, 0 errors, 1 failures, 0 skipped

Summary: 201 tests, 0 errors, 0 failures, 0 skipped

For what it's worth, we've been using these speed limiter functions on Ignition Gazebo, and while writing tests for it today, I noticed there was a mistake on the jerk calculation. The change proposed here fixed my tests in gazebosim/gz-math#194.

This LGTM, glad to hear that the issue has been identified by others and the fix has been validated there too. Plus, the math makes sense.

I think part of the reason this got mixed up at first is because of the variable naming. da/da_min/da_max are pretty poor names IMO, really those terms are d²v.

I think it becomes much clearer as follows, though I admit this does take a few extra cycles since it divides.

const double a  = (v  - v0) / dt;
const double a0 = (v0 - v1) / dt;
  
const double da = clamp(a - a0, min_jerk*dt, max_jerk*dt);
v = v0 + (a0 + da)*dt;